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1. Introduction 

Trajectory planning algorithms seldom incorporate extensive knowledge of the interaction 
between inverse dynamics and actuator torque limits into the planning process. Past efforts have 
typically used fixed velocity limits of the joints as a way of determining how fast a trajectory 
may be executed [8], Due to the complex relationship between joint velocities and dynamics, 
such a procedure is at best a very coarse approximation of the true influence of actuator limits 
on trajectory speed. An exact method for determining the optimal velocity distribution for 
a fixed path has been proposed in [13], where dynamic programming was straightforwardly 
applied to minimize energy under actuator and dynamic constraints. The computational cost of 
such optimization approaches however may prevent their useful application. 

We develop a fundamental time-scaling property of manipulator dynamics that allows 
trajectory planning and inverse dynamics to be exactly and efficiently coupled. The dynamic 
realizability of a proposed trajectory can be readily determined, and a simple procedure to 
modify the movement speed can be applied to render proposed trajectories realizable. 

We presume that a time sequence of joint angles 0(f) = (0i(t), 8 2 (t),..9 n (t)) for an n-joint 
manipulator has been proposed by the trajectory planner, where t represents the time in the 
interval 0 < t < t/. Because of fast recursive formulations of inverse dynamics [4, 7, 11], for 
each sampling time t the joint torques n(t) = (ni(f), n 2 {t), ■ ■n n (t)) corresponding to 0(f) 
can be efficiently found. The comparison of n(f) against motor torque limits is therefore readily 
accomplished, and it is straightforward to determine whether the proposed trajectory can be 
realized by the actuators. 

A more difficult task is to ascertain how to change tire trajectory in case motor torque limits 
are violated. Here we consider only changing the speed at which a manipulator follows a path, 
where by speed change is meant a constant scaling of the velocity profile so that the total 
movement duration is scaled without changing the actual path through space. It is not sufficient 
merely to slow down a trajectory, with the hope that a slower trajectory requires lower motor 
torques, because some trajectories can only be realized at higher speeds, and some trajectories 
may not be realizable at any speed. Moreover, unless one is careful to employ an algorithm such 
as is presented here, then modifying the movement speed requires that the inverse dynamics be 
recomputed from scratch. 

The algorithm presented here determines what speed range is permissible for the proposed 
trajectory given actuator torque limits. At the same time the nominal dynamics for the proposed 
trajectory can be simply modified for the new trajectory, without dynamics recomputation. 

2. Time Scaling and Trajectories 

Suppose that some trajectory plan 0(f) has been fashioned. A new trajectory 0(f) will be defined 
such that 0(f) = 0(r), where r = r(f) is a monotonically increasing function of time with 
r(0) = 0 and r(fi) — t/ for some t\ > 0. The function r(t) can be considered a time warp 
which moves the arm along the same path but with a different time dependence, perhaps going 
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slower along some points of the path and faster along others. r(t) must increase monotonically 
because time cannot reverse itself, and r(0) = 0 because the movement must start at tire same 
point. 

. To determine how tire dynamics of the arm changes for the new trajectory, the time derivatives 
of the joint angles are required. From the chain rule, 

d0(t) d8(r) dr 

dt dr dt 

or, using the dot notation for time derivatives, 

0(f) = i( r )r (1) 

where dd(r)/dr has been written 6{r) because it takes the value 0 evaluated at r(t). Similarly, 

The dynamic equations of motion can be compactly written [4] as 

n(t) = l(£(i))i(0 4- 0 {t) ■ C(£(«)) • 0{t) -f g(£(<)) (3) 

where 

n(f) is the n-dimcnsional vector of net joint torques corresponding to the movement 
point, 

l(0(t)) is the n X n generalized inertia tensor of the manipulator, 

C(8(t)) is the n X n X n position-dependent tensor in the formulation of tlte Coriolis 
and centripetal torques, and 

g (0(t)) is the position-dependent n-dimensional vector of gravity torques. 

The notation for the velocity product term 0 • C ■ £ is slightly unconventional, but has been 
adopted for compactness. The product C -0 is an n X n matrix with clement ij as Ylk CijkQk, 
which in turn is multiplied against 0 to yield an n X 1 vector. 

In the following derivations, die acceleration and velocity dependent torques are treated 
separately and are designated as n Q (i) = (n a i(t), n a v{t), ...,n an (t)), so that n(i) = n Q (f) + 
g(^(t)). For the new trajectory 0(t), 

n a (t) - I(I(i))2(i) + h) ■ C(m ■ ~0(t) (4) 

Substituting from (1) and (2), 

n«(<) = I(£(r))£(r) + l{ r ) ■ C(g(r)) • 0{r) f 2 + I (0(r))0(r)r (5) 

Rearranging and substituting from (3), 


n o (0 = r 2 n a {r) + rI(0(r))O(r) 


(6) 
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This is a potentially significant reformulation of dynamics, indicating how tire underlying 
dynamics changes when the time dimension of a trajectory changes. The new torque n tt (f) is 
related to the old n a (r) by the scaling factor f 2 plus a term proportional to the generalized 
momentum l(0(r))0(r) of-the manipulator. Note that the gravity torque g(0(f)) = g(0(r)) is 
not scaled since it is position dependent only, which is the reason for the separation between 
n a (£) and tire gravity torques. 

2.1 Constant Time Scaling 

The simplest instance of (7) is when r(t) — 0, i.e., r(t) = ct for some constant c > 0. If 
c > 1 tire movement is sped up; if c < 1 tire movement is slowed down. Then 

n a (£) = c 2 n a (ct) (7) 

Interestingly, movement speed can be proportionally changed without affecting the underlying 
dynamics very much, so long as the gravity contribution is separated from the acceleration and 
velocity term contributions. The relation was also noted by Bejzcy [I], Humans apparently adopt 
such a strategy when changing movement speed, perhaps to simplify the dynamics computation 

[5], 

This relation also shows that the velocity and acceleration terms of the dynamics would have 
the same significance relative to each other for all speeds of movement. For, the acceleration 
term I(0(t))0(i) is scaled by c 2 from (2), and die velocity term £(£) • C(0(f)) • |(f) receives a 
c factor for each 0(f). Thus both terms change equally with differing movement speeds. This 
contradicts the normal assumption in the robotics literature, where in designing control systems 
workers typically throw out tire velocity terms because they are a nonlinear product, with the 
presumption that they are significant only at higher speeds of movement [1,10]. For the slow 
movement speeds of most manipulators, and hence because of the predominance of frictional 
and gravitational effects, this is a reasonable assumption [2]. But for consistency tire acceleration 
terms should be thrown out as well since they share the same significance as tire velocity terms, 
yet this is not done. In any case, future generations of robots will contain examples of fast 
manipulators with low joint friction where dynamic effects, both acceleration and velocity terms, 
are highly significant [2], 

In the remainder of this paper, we assume the special case (7) and use it to determine 
allowable speeds of movement for a given trajectory. By allowable speed it is meant that the 
trajectory is stretched or compressed uniformly to fit the allotted duration without changing 
the path or the velocity profile shape. Constant scaling of velocity is a simple but important 
method of bringing a trajectory within actuator constraints. Certainly there are many classes of 
manipulator trajectories where an exact path through space must be followed, as in straight-line 
Cartesian motions of the manipulator hand [9, 12], but where the time dependence along the 
path is not strongly restricted. While non-uniform time scaling may yield a realizable trajectory 
where a constant scaling would not, results for the general case (6) are not yet available while 
other approaches [13] may be too computationally inefficient for routine use. Even more difficult 
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is path modification under actuator and dynamic constraints, for which no general results are 
yet available (see however [6] for an approximate time-optimal trajectory planning solution). 

3. Time Scaling of Trajectories to Satisfy Torque Limitations 

Torque limits of actuators restrict how fast a manipulator may move along a trajectory. In 
order to determine whether a proposed trajectory 8(t) violates actuator limits, die inverse 
dynamics must be solved and the computed torques compared to dicse limits. Suppose we 
have computed the acceleration and velocity dependent torques n 0 (f) separately from the 
gravitational torques g {0{t)). Suppose further diat die maximum and minimum torque limits, 
n+ = (n^,n 2 ~,...,n+) and n — = ..., n~) respectively, are constant diroughout 

a movement. (Ordinarily one would presume n+ = —n — .) Later we consider velocity 
dependencies as in electric torque motors. 

At a given position 8(t) of the manipulator, some of the actuator torque is required for 
postural support of die manipulator only. Jn terms of what torque capability is remaining 
to actually generate a movement, we formulate new effective torque limits by absorbing the 
gravitational torques into the torque limits, i.e., 

n+(t) =n+ - g(8(t)) 

n~(*) — n~ - g(£(<)) 

/'""■s, Note that the torque limits are now position dependent, and hence have been written as 

functions of time. 

Because we are looking for a time scaling value c that brings the trajectory within the 
torque limits, a slight alteration of (7) is required. Since (7) holds for all times, we can write 
n a {t/c) — c 2 n a (t) and the torque limits for the new trajectory as n^jt/c) = n±(t). We require 
that for the new trajectory n a [tjc) be bounded by n±(t/c), which is done by finding the c that 
bounds c 2 n a (f) by n±(t) according to die following procedure. 

For each time t and joint i, we find the minimum and maximum scaling values of c 2 that 
satisfy the torque limits by solving (7) together with the computed torques n a i[t) and the torque 
limits n~(t) and n±(t). The result will be denoted by the interval [c? (t), (t)], where any 

scaling value within this interval is a permissible movement speed for this joint at diis point in 
the trajectory. This scaling interval, however, may violate constraints at other joints and times, 
and the permissible range of c 2 values for die whole movement is found by intersecting all such 
intervals: 

F“~. c 2+ ) = OLW. (9) 

i,t 

Wc can then choose any value in the final interval [c 2 ~, c 2 +] to generate a movement which 
satisfies the actuator constraints. 

To determine [c 2 ~'(t), c^ + (f)J, dicre are three cases. 
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Case 1: nf(t) > 0, n i ( t ) < 0. 

Condition c 2 ~(f) c^(f) 

n ai [t) >0 0 nf(t)/n ai (t) 

n ai {t ) = 0 0 oo 

n ai {t) <0 0 n~{t)/n ai (t) 

Case 2: n^~[t) > 0, n~(t) > 0. 

Condition c 2— (f) cf~*~(£) 


n ai (t) >0 n i ( t)/n ai {t ) nf~(t)/n ai (t) 

n ai (t) < 0 unrealizable 

Case 3: n^[t) < 0, n~{t ) < 0. 


Condition 

n ai (t) > 0 
n a i(t ) < 0 


stM 

unrealizable 

n+(t)/n ai {t) 


c- + (t) 


n i M/ n aiW 


To explain these cases, consider first case 1, The minimum value of c 2 is zero because 
n a i{t) = 0 falls within actuator bounds and c 2 must be non-negative. If > 0, then 

the appropriate torque limit for comparison is n^(f), because time scaling can change a 
torque magnitude but not a sign. The maximum value of c 2 is then determined by the ratio 
n a i[t). Note that when n ai (t) < n^(t), then c 2 > 1 and it is possible to speed the 
movement up and still satisfy actuator constraints. When n at (f) > n+(f), c 2 < 1 and the 
movement must be slowed down. To complete case 1, if n a i(t) < 0, the appropriate torque 
limit is n~(t) and the maximum value of c 2 is n~(t)/n a i(t). 

In case 2, if n at (f) < 0, then this movement is unrealizable at any speed. The actuator 
can produce only a positive torque, but a non-positive torque is required by the movement. 
Put simply, the manipulator cannot even hold itself up at this position. Of course manipulator 
actuation is ordinarily designed to counteract gravity, but this actuation may become inadequate 
if too heavy a load is picked up. For n a i(t) > 0, the maximum movement speed is determined 
by the ratio nf'(t)/n a i(t) and the minimum by n~(t)/n ai (t). It is possible that c 2 ~~(t) > 0, 
which says that there is a minimum non-zero speed at which die movement is realizable. Also 
it is possible that c 2— (f) > 1, so that the movement can be realized only by speeding up. Case 
3 is analogous to case 2, except diat the roles of and n~(t) are reversed due to sign 
change. 

The intersection of all the intervals [c 2— (t), c^(f)] may be null, with incompatible scaling 
requirements at different parts of the trajectory. This movement is then unrealizable at any 



7 


R 



Figure 1 

A planar two-link manipulator. 


speed. If c 2 ~ > 1, then the movement should be speeded up by at least a factor c 2 ~, while 
if c 2+ < 1 the movement should be slowed by at least a factor c 2 + in order to produce a 
realizable trajectory. Having chosen a c 2 value, the inverse dynamics can be simply recomputed 
from the old values of n 0 (t) and g(0(4) as follows: 

n(t) = c 2 n u (ct) + g (9(d)) (10) 

The acceleration and velocity torques arc amplitude scaled, die gravity torque is added in 
separately, and both together arc time scaled. Speed change can therefore be accomplished 
without dynamics recomputation. 

4. Examples 

This algorithm will be illustrated for straight-line movements by a two-link planar manipulator 
(Figure 1); the algorithm is quite easily applied to manipulators with more degrees of freedom. 
This manipulator has two rotary joints with joint angles 9 1 at the shoulder and 9 2 at the elbow. 
The axes of rotation are both directed along the 2 -axis, so that the manipulator only generates 
movement in the x-y plane. Gravity is presumed to act in the negative y direction with 
magnitude g. The length, mass, and moment of inertia about the proximal joint for each link 
are designated as /», m;, and 4 respectively, where i — 1 refers to the upper arm link and 
i = 2 refers to the forearm link. Each link is a uniform cylinder with radius R. 

The equations of motion arc [2]: 


a ( T , m-zhh n 
ni = OA h 4--— cos 0 2 + - 


) 


+ 02 h + 


m 2 lV 


m.2+: 


-9j sin 0 2 4 


m 2 hg 


cos (0i + 9 2 ) 


2 


2 


( 11 ) 



ni = 0 i h + h + rmhh cos 0 2 + 


mi/j + ^2^2 


+ m 2 l\ 


+h 




m 2 l\l 2 

_____ 



(12) 


— r n zli h q2 g j n ^ _ jn, 2 / 1 / 2 0 1 ^ 2 sin 02 
<6 

+(^“ cos (01 + 0 2 ) + h (j~ + rn 2 j cos O^jg 

A common class of manipulator trajectories are straight line movements of the tip, i.e., 
V — 2/o = (s — z 0 )(yi ~ Vo)/[xi — io) for beginning and end positions of the tip (xo,2/o) 
and [x\,yi) respectively. To solve the inverse dynamics, it is required to transform from the 
position, velocity, and acceleration of the tip to the position, velocity and acceleration for each 
joint angle. These inverse kinematic equations arc presented below [2]: 



x 2 4- y 2 - 



cos d 2 

2/1/2 


01 

= tan -1 ( - 

j tan”T 7 h Y 9 \ ) 



\x 

) V Zl "T ^2 COS 0 2 / 


01 ‘ 

1 

h cos(0i -f 0 2 ) /2 sin(0i + 0 2 )" 

X 

01 + 02 

kh sin 02 

—l\ cos 0i —/1 sin 

.y. 

' 0i,. 

1 

h cos(0i + 02) h sin(0i + 02) 

\x 

01 4- 02 

/1Z2 sin 02 

— Zicos0i —/1 sin 

[y. 


(13) 

(14) 


+ 


1 

’/1/2 COS 02 

ii 

r 0? ^ 

/i / 2 sin 02 


—hh cos 0 2 . 

,(0i 4- 0 2 ) 2 . 


(15) 


Three different movements are illustrated in tire examples below: one that must be slowed 
down, one that must be sped up, and one that is unrealizable at any speed. For the link 
parameters, we have set /1 = l 2 — 0.5 meters, mi = m 2 — 1 kg, h = I 2 = mi/f/12 -j- 
miR 2 /4, R = O.l/i, and g — 9.8 m/sec 2 . 


4.1 A Movement Whose Speed Is Scaled Down 


A straight line motion from ( 10 , 2 / 0 ) = (0.5,—0.5) to (xi,yi) = (0.5,0) is to be generated 
at a constant velocity of 4 meters/second. The torque limits for the actuators are set at — 
—= 6.9 kg-m, and = 2 kg-m. A comparison between n+(t), n~(i), and 

n a (t) is presented in Figure 2. 
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Figure 2 

Torque profiles for a constant-velocity, straight-line trajectory of 4 m/sec from (x,y) = (.5,—.5) to (.5,0) 
when nf — —n~ — 6.9 kg-m and = —nj" = 2 kg-m. (a) Joint 1 minimum and maximum torques, 
«f(f) and and the velocity and acceleration torque n a \{t) are shown versus time; (b) corresponding 
torque profiles for joint 2. 


Joint 1 is represented by Figure 2a, where die torque requirements n a \{t) for the complete 
movement fall within the modified actuator constraints n[~(f) and n^t). For joint 2 in Figure 
2b, however, tire required torque n a 2 (t) falls below the lower actuator bound for the 

initial movement segment. This suggests that the movement must be slowed down. By scaling 
the torque n a 2 {t) by a factor c 2 < 1, the elements of the new torque n a 2 (f) become larger 
(i.c., less negative). The n o2 (t) curve could then be made to lie completely above the njf (£) 
curve, as if it had been shifted upwards. 

Carrying out the computations in (9), it is found that [c 2— , c 2 +] = [0.582,0.745]. The value 
c 2 + = 0.745 arises from joint 2 at time t = 0.035 sec., while the value c 2 “ = 0.582 arises 
from joint 1 at tire same time. Thus the fastest speed at which this movement can be executed 
is determined by 4\/0.745 = 3.45 m/s. On the other hand, there is a non-zero lower speed 
limit, 4\/.582 = 3.05 m/s. Examining Figure 2a, if the movement is slowed too much, then 
the n al {t) curve is displaced upwards, intersecting the n^(t) curve and exceeding that upper 
torque limit. 

4.2 A Movement Whose Speed Is Scaled Up 

As shown above, if the movement speed falls under 3,05 m/s, then the actuator limits are 
exceeded. This condition is verified here by considering the same movement but executed 
at 2 m/s and by working through the algorithm. Figure 3a show's that the shoulder torque 
n a i(t) exceeds the upper actuator bound n^(t) at the beginning of the movement. Calculations 
show that for joint 1, c 2 ~ = 2.329 at t = 0.035 s, so that the movement must be sped up 
by 2\/2.329 = 3.05 m/s as predicted. This would push the n a i(t) curve down until it is 
completely beneath n^lt). The curve nj~(£) is the same as in Figure 2a, but has been left out 
here to allow an expanded scale. 

There is an upper limit of c 2 + = 2.981 at t — 0.035 s as well, determined this time by 
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Figure 3 

Torque profiles for a constant-velocity, straight-line trajectory of 2 m/sec with oLher conditions the same 
as in Figure 2. 



Figure 4 

Joint 2 torque profiles for the same movement as in Figure 3 except with nf = —nj* = 1 kg-m. 


joint 2 (Figure 3a). If the curve n a 2 {t) is pushed down too far, it will violate the lower bound 
njf (f). Thus the fastest this movement can be executed is 2\/23J8T = 3.45 m/s, in agreement 
with the first movement analysis. 

4.3 An Unrealizable Movement 

For the third movement, the conditions arc the same as for the second movement, but the 
second actuator limits are now changed to = —n^~(f) — 1 kg-m. As before, the actuator 

limits on joint 1 (Figure 3a) require that the minimum speed for this movement be determined 
by c 2 ~ = 2.329. But Figure 4 shows that the joint 2 actuator limits prevent any higher speed 
scaling than c 2 + = 1.522, because n a 2 (t) would fall below n^(f). Thus there are incompatible 
scaling requirements, and this movement cannot be realized at any speed. 

5. Velocity-Dependent Motor Limits 

We have assumed above that the actuator limits n+ and n — are constant throughout the motion. 
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In reality, the maximum actuator torque often does depend on velocity, such as for electric 
torque motors. For the case of a low-inductance motor with no dissipative effects [3], 

V = IR + K v u> ' (16) 

where V is the motor voltage, I is the motor current, R is the motor resistance, o> is the 
rotational speed of the motor, and K v is a constant of proportionality for the back-EMF term. 
The torque n produced by the motor is directly proportional to current: 

n = K n I (17) 

In voltage-control mode, for example in a chopping circuit where duty cycle is modulated, 
there is an upper voltage limit V max which can be applied to the motor. Because of the back- 
EMF, the maximum current, and hence tire maximum torque, is velocity dependent. Assuming 
a gear ratio of 1 (otherwise absorb the gear ratio into K v ), then ui = 6, where 0 is the velocity 
of the joint actuated by the motor. Combining (16) and (17), and absorbing tire gravity torque 
g(0{t)) into the motor limit, 

n+(f) = ^(V max - Kj(t)) - g(9(t)) (18) 

If the trajectory is to be time-scaled by a factor c, then 

« + (*) = ~V max — Kr \^ v cd(ct) — g{0{ct)) (19) 

ri ri 

where unlike the dynamic terms there is a linear dependence on tire scale factor c. When solving 
for the scaling that satisfies the upper bound, again we need the relations h a (t/c) — c 2 n a (t ) 
and n+(f/c) = n+(t). There is a quadratic equation in c when h+(t/c) is replaced by c 2 n a (t) 
in (19): 

n a (t)c 2 + ^~-c0(t) - ~^V max + g(6(t)) - 0 (20) 

ri Jri 

When solved, 


- ( 21 ) 

The root which gives the largest positive c should be chosen for c 2 +. As before, it is possible 
that there is no positive (or even real) root, which indicates that the trajectory is unrealizable. 
We may also solve (21) with V m i n to find c 2 ~. The procedure for determining the appropriate 
trajectory scaling factor then follows that indicated by (9). 

The back-EMF can be considered a form of viscous friction, but if there were any additional 
viscous friction at a joint or actuator, it could be handled in the same manner. As regards 
Coulomb, or sliding, friction, it could be subtracted from the motor torque limits depending 





on the direction of movement, i.e., sliding friction torque = — njsgn(0(t)). Any actuator 
springiness, which is position-dependent, could also be readily absorbed into the torque limits. 
Motor inductance unfortunately seems to present an intractible problem, due to the need to 
find tire time derivative of the dynamic equation (3). 

Conclusion 

Trajectory planning and inverse dynamics may be efficiently coupled to reflect the exact influence 
of actuator torque limits on execution capability. By factoring out gravity, a time-scaling property 
of manipulator dynamics readily allows a realizable speed of movement for the whole trajectory 
to be determined if there exists one. Rather than recomputing the dynamics corresponding to 
a new trajectory speed from scratch, the dynamics of the new trajectory is obtained by a simple 
linear combination of components of the original trajectory dynamics. 

Velocity-dependent actuator limits, as well as various sources of joint friction, can be 
accommodated in this scheme. An important side effect of the dynamic time scaling property is 
that a ubiquitous assumption in manipulator control, namely that the velocity-product dynamic 
terms are significant only at high speeds of movement, is false: these terms have the same 
significance relative to the acceleration dynamic tenns for all speeds of movement. 
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